#include "sensor.h"
#include "math.h"
float offset = 0.0;
float last_valid_offset = 0.0f;
uint8_t sensor_arry[9]={0,0,0,0,0,0,0,0,0};//传感器数组
uint8_t square_state = 0; //1-完成
uint8_t Round = 1; //循环圈数
uint8_t count = 0;

void sensor_read(uint8_t *sensor_arry)
{
    if(sensor_arry == NULL) 
			return; // 检查指针是否为NULL
	    
		sensor_arry[1] = (HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_14) == GPIO_PIN_SET) ? 1 : 0; // 左3
		sensor_arry[2] = (HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_15) == GPIO_PIN_SET) ? 1 : 0; // 左2
		sensor_arry[3] = (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2) == GPIO_PIN_SET) ? 1 : 0; // 左1
		sensor_arry[4] = (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_3) == GPIO_PIN_SET) ? 1 : 0; // 中间
		sensor_arry[5] = (HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_1) == GPIO_PIN_SET) ? 1 : 0; // 右1
		sensor_arry[6] = (HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_0) == GPIO_PIN_SET) ? 1 : 0; // 右2
		sensor_arry[7] = (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_8) == GPIO_PIN_SET) ? 1 : 0; // 右3
	}

	void Sensor_TaskWrapper(void)
	{
	    sensor_read(sensor_arry); // 读取传感器状态
	}
    
void motor_speed_set(int16_t left_speed, int16_t right_speed)
{	    	
	if(left_speed >=0)
	{
	// 电机方向的设置
		MOTOR_A_FORWARD();  // 左电机	
	}
	else if(left_speed <0)
	{
		left_speed=-left_speed;
		MOTOR_A_BACKWARD();
	
	}
	if(right_speed >=0)
	{
		MOTOR_B_FORWARD();  // 右电机
	}
	else if(right_speed <0)
	{
		right_speed=-right_speed;
		MOTOR_B_BACKWARD();  // 右电机

	}
	// 设置电机速度
	Motor_SetSpeed(1, left_speed);   // 1是左电机
	Motor_SetSpeed(2, right_speed);  // 2是右电机

	    
}  
// 巡线PID参数
void sensor_track_PID_Init(void)
{
	sensor_track_PID.Kp = 0.00f;
	sensor_track_PID.Ki = 0.00f;
	sensor_track_PID.Kd = 0.00f;
	sensor_track_PID.integral_max = 50.0f;
}

//计算七路传感器偏移量。 其中1代表黑线，0代表白色地图背景，数组下标从0开始
float cal_sensor_offset(void)
{
    uint8_t sum = 0;
    const float weights[8] = {0, 1.0f, 1.1f, 1.3f, 1.5f, 1.3f, 1.1f, 1.8f};
    
    for (int i = 1; i <= 7; i++)
    {
        sum += sensor_arry[i];
    }
    uint8_t black = sum;
    
    if(black == 0)
    {
        offset = -2.0f;
        return offset;
    }
    
    // 处理中间区域检测到黑线的情况优先直行
    if(sensor_arry[3] || sensor_arry[4] || sensor_arry[5])
    {
        float center_offset = 0.0f;
        uint8_t center_count = 0;
        
        if(sensor_arry[3]) 
        {
            center_offset += -1.0f;  // 左偏
            center_count++;
        }
        if(sensor_arry[4]) 
        {
            center_offset += 0.0f;   // 中心
            center_count++;
        }
        if(sensor_arry[5]) 
        {
            center_offset += 1.0f;   // 右偏
            center_count++;
        }
        
        if(center_count > 0) 
        {
            center_offset /= center_count;
            
            // 弱化两侧影响
            float side_effect = 0.0f;
            if(sensor_arry[1]) side_effect -= 0.5f;
            if(sensor_arry[2]) side_effect -= 0.3f;
            if(sensor_arry[6]) side_effect += 0.3f;
            if(sensor_arry[7]) side_effect += 0.5f;
            
            offset = center_offset + side_effect;
            return offset;
        }
    }
    
    if((sensor_arry[6] || sensor_arry[7]) && 
       !(sensor_arry[3] || sensor_arry[4] || sensor_arry[5]))
    {
        // 根据右侧传感器位置计算偏移量（正值表示向右调整）
        float right_offset = 0.0f;
        uint8_t right_count = 0;
        
        if(sensor_arry[6]) 
        {
            right_offset += 1.2f;  // 右2检测到
            right_count++;
        }
        if(sensor_arry[7]) 
        {
            right_offset += 2.2f;  // 右3检测到
            right_count++;
        }
        
        if(right_count > 0) 
        {
            offset = right_offset / right_count;
            
            if(offset > 2.8f) offset = 2.8f;
            return offset;
        }
    }
    
    // 处理左侧
    if(sensor_arry[1] && !(sensor_arry[3] || sensor_arry[4] || sensor_arry[5])) 
    {
        offset = -2.8f;
        return offset;
    }
    
    float total = 0.0f;
    float weight_sum = 0.0f;
    
    for (int i = 1; i <= 7; i++) 
    {
        if (sensor_arry[i] == 1) 
        {
            float w = weights[i];
            total += (i - 4) * w; // i-4将中心设为0
            weight_sum += w;
        }
    }
    
    if(weight_sum > 0)
    {
        float offset = total / weight_sum;
        
        if(offset > 2.8f) offset = 2.8f;
        if(offset < -2.8f) offset = -2.8f;
        return offset;
    }
    if (sensor_arry[1] || sensor_arry[2]) 
	{
        last_valid_offset = -1.0f; // 左偏标记
    }
    else if (sensor_arry[6] || sensor_arry[7]) 
	{
        last_valid_offset = 1.0f;  // 右偏标记
    }    
    return offset; // 默认值
}

/*-----------------------------------------循迹任务-------------------------------------------------*/
// 直线行驶
void square_straight(void)
{
	float offset = cal_sensor_offset();
	float pid_output = PID_Calc(&sensor_track_PID, 0.0f, offset, 1);
	
	int16_t base_speed = 342;  // 降低基础速度，提高稳定性
	int16_t left_speed = base_speed - pid_output;
	int16_t right_speed = base_speed + pid_output;
	
	motor_speed_set(left_speed,right_speed);
	
//	printf("PID:%d\n",left_speed );

}


void square_tracking_task(void)
{	
	
	static uint8_t was_white = 0; 
	uint8_t white = 1;
	for(int i=1;i<=7;i++)
	{
		if(sensor_arry[i] !=0)
		{
			white = 0;
			break;
		}
	}
	if(was_white && !white)
	{

		count++;
//		printf("count%d\n",count);
		was_white=0;

	}
		was_white = white ;
	if(sensor_arry[3] || sensor_arry[4] || sensor_arry[5]||sensor_arry[6])
	{
		square_state = 0; //行走状态
		square_straight();
	}
	else
    {
			Yaw_target = Yaw + 90.0f;
			jy62_proc();
    }
//	if(Round == 1)
//	{
//		if(count - 1 == 4)
//		{
//			square_state = 1;
//			motor_speed_set(0,0);
//		}
//	}
//	else if(Round == 2)
//	{
//		if(count - 1 == 8)
//		{
//			square_state = 1;
//			motor_speed_set(0,0);
//		}
//	
//	}
//	else if(Round == 3)
//	{
//		if(count - 1 == 12)
//		{
//			square_state = 1;
//			motor_speed_set(0,0);
//		}
//	
//	}
//	else if(Round == 4)
//	{
//		if(count - 1 == 16)
//		{
//			square_state = 1;
//			motor_speed_set(0,0);
//		}
//	
//	}
//	else if(Round == 5)
//	{
//		if(count - 1 == 20)
//		{
//			square_state = 1;
//			motor_speed_set(0,0);
//		}
//	
//	}	

}














